#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h"
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sstream>

#define DEST_PORT 22897
#define DSET_IP_ADDRESS "121.199.22.135"

/* socket文件描述符 */
int sock_fd;
struct sockaddr_in addr_serv;
int len;
int cnt=0;/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void chatterCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	cnt++;
	if(cnt%15!=0){return;}
	ROS_INFO("I heard: [%f] [%f]", msg->pose.position.x,msg->pose.position.y);
	std::ostringstream s1, s2, s3;
	s1 << msg->pose.position.x<<","<< msg->pose.position.y<<"," << msg->pose.position.z<<","
	<< msg->pose.orientation.x<<","<< msg->pose.orientation.y<<"," << msg->pose.orientation.z<<"," << msg->pose.orientation.w;
	// std::string x(s1.str()), y(s2.str()),z(s3.str());
	// std::string s = "{\"type\":200,\"data\":{\"x\":" + x + ",\"y\":" + y + ",\"z\":"+z+"}}";
	std::string s=s1.str();
	std::cout<<s<<std::endl;
	int send_num = sendto(sock_fd, s.c_str(), strlen(s.c_str()), 0, (struct sockaddr *)&addr_serv, len);

	if (send_num < 0)
	{
		perror("sendto error: not connect");
		// exit(1);
	}
	cnt=0;
}

int main(int argc, char **argv)
{

	/* 建立udp socket */
	sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
	if (sock_fd < 0)
	{
		perror("socket");
		// exit(1);
	}

	/* 设置address */
	
	
	memset(&addr_serv, 0, sizeof(addr_serv));
	addr_serv.sin_family = AF_INET;
	addr_serv.sin_addr.s_addr = inet_addr(DSET_IP_ADDRESS);
	addr_serv.sin_port = htons(DEST_PORT);
	len = sizeof(addr_serv);


	ros::init(argc, argv, "listener");
	ros::NodeHandle n;

	ros::Subscriber sub = n.subscribe("ndt_pose", 10, chatterCallback);

	ros::spin();

	return 0;
}
